#include "AI.h"
#include "Physics/Box2DVehicle.h"
#include "Body.h"
#include "System/GameObject.h"

mtBEGIN(AI)
	mtBASE(ECS::Component);
	mtMETHOD(onUpdate);
mtEND

class MoveByPathSteering : public ISteering
{
public:
    MoveByPathSteering(IVehicle* vehicle, std::vector<MapTile>& path) :
        _vehicle(vehicle),
        _path(path),
        _index(0)
    {

    }
    virtual Vector getSteering() override
    {
        if (_index >= _path.size())
            return Vector(0, 0);

        Vector target(_path[_index].x, _path[_index].y);
        Vector position(_vehicle->getPosition());
        Vector distance(target - position);
        while(_index < _path.size() && distance.length() < 0.1f)
        {
            ++_index;
            if (_index < _path.size() )
            {
                target.x = _path[_index].x;
                target.y = _path[_index].y;
                distance = target - _vehicle->getPosition();
            }
        }

        Vector desizredVelocity = distance.normalize() * (_index == _path.size() - 1 ? 0 : _vehicle->getMaxSpeed());
        Vector steering = desizredVelocity - _vehicle->getVelocity();

        return steering;
    }

    IVehicle* _vehicle;
    std::vector<MapTile> _path;
    int _index;
};

void AI::onUpdate( float dt )
{
    if (mSteering)
    {
        Vector v = mSteering->getSteering();
        Vector force = v.truncate(2.0f);
        float a = -atan2(v.x, v.y);
        mVehicle->setAngle(a);
        //mVehicle->applySteering(v);
        mVehicle->applyForce(force);
    }
    else if (!_path.empty())
    {
        GameObject* go = (GameObject*)getAssembly();
        MapTile start(go->getPosition().x, go->getPosition().y);
        MapTile end = _path[0];

        auto* steering = new ArrivalSteering();
        steering->init(mVehicle, Vector(end.x, end.y), 0.1f);
        setSteering(steering);

        _path.erase(_path.begin());
    }
}

void AI::onStart()
{
    Body* cachedBody = getAssembly()->getComponent<Body>();
    mVehicle = new Box2DVehicle(cachedBody->getBody());
}

void AI::setPathTo( int x, int y )
{
    GameObject* go = (GameObject*)getAssembly();
    MapTile start(go->getPosition().x, go->getPosition().y);
    MapTile end(x, y);
    World::Instance()->getMap()->findPath(start, end, &_path);
    setSteering(new MoveByPathSteering(mVehicle, _path));
}

void AI::setSteering( ISteering* steering )
{
    if (mSteering)
        delete mSteering;
    
    mSteering = steering;
}

AI::AI() :
    mVehicle(nullptr),
    mSteering(nullptr)
{

}
